/*
 * app_main.c
 *
 *  Created on: Nov 18, 2024
 *      Author: Vinson
 */


#include "app_main.h"
#include "global_var.h"
#include "main.h"
#include "gpio.h"
#include "stdio.h"
#include "tle5012b.h"
#include "drv832x.h"
#include "sensor.h"
#include "mc_math.h"

gpio_t led_r = { .group = LED_R_GPIO_Port, .num = LED_R_Pin };
gpio_t led_g = { .group = LED_G_GPIO_Port, .num = LED_G_Pin };
gpio_t sen_cs = { .group = SEN_CS_GPIO_Port, .num = SEN_CS_Pin };
gpio_t drv_en = { .group = DRV_EN_GPIO_Port, .num = DRV_EN_Pin };
gpio_t drv_cs = { .group = DRV_CS_GPIO_Port, .num = DRV_CS_Pin };
gpio_t dbg_out2 = { .group = DBG_OUT2_GPIO_Port, .num = DBG_OUT2_Pin };

//lowpass_filter_t *m0_filter;
lowpass_filter_t *m0_lpf_angle;
lowpass_filter_t *m0_lpf_vel;
lowpass_filter_t *m0_lpf_current;
pid_control_t *m0_speedPid;
pid_control_t *m0_currentPid;
pid_control_t *m0_anglePid;

int isReadyOn = 0;

static void adc_init()
{
	/*Step1：设置采样 6.5 周期 */
    LL_ADC_SetChannelSamplingTime(hadc1.Instance, LL_ADC_CHANNEL_1, LL_ADC_SAMPLINGTIME_6CYCLES_5);
    LL_ADC_SetChannelSamplingTime(hadc1.Instance, LL_ADC_CHANNEL_2, LL_ADC_SAMPLINGTIME_6CYCLES_5);
    LL_ADC_SetChannelSamplingTime(hadc1.Instance, LL_ADC_CHANNEL_3, LL_ADC_SAMPLINGTIME_6CYCLES_5);
    LL_ADC_SetChannelSamplingTime(hadc2.Instance, LL_ADC_CHANNEL_1, LL_ADC_SAMPLINGTIME_6CYCLES_5);
    LL_ADC_SetChannelSamplingTime(hadc2.Instance, LL_ADC_CHANNEL_2, LL_ADC_SAMPLINGTIME_6CYCLES_5);
    HAL_ADCEx_Calibration_Start(&hadc1, ADC_SINGLE_ENDED);
    HAL_ADCEx_Calibration_Start(&hadc2, ADC_SINGLE_ENDED);
    HAL_ADC_Start(&hadc1);
    HAL_ADC_Start(&hadc2);
    HAL_ADCEx_InjectedStart_IT(&hadc2);
    HAL_ADCEx_InjectedStart_IT(&hadc1);
}

static void spi_init()
{
#ifdef TLE_5012B_DMA
    /*Timer CC4 重置CS 位 */
	printf("spi dma init\n");
    static uint16_t gpo_tx_val = SEN_CS_Pin;
    HAL_DMA_Start(htim1.hdma[TIM_DMA_ID_CC4], (uint32_t)&gpo_tx_val, (uint32_t)&SEN_CS_GPIO_Port->BRR, 1);
    __HAL_TIM_ENABLE_DMA(&htim1, TIM_DMA_CC4);

#endif
}

static void timer_init()
{
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 2048);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 2048);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 2048);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 250); // >= 1, ```|_|``` trigger on neg-edge, sensor
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_5, 12); // >= 1, ```|_|``` trigger on neg-edge, adc
    // adc 3.5 cycles @ 170M/4 -> 82.35 nS -> /2 -> 41.2 nS, pwm 12 -> 70.6 nS /1

    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);

#ifdef TLE_5012B_DMA
    __HAL_TIM_ENABLE_IT(&htim1, TIM_IT_CC4);

    /* 使能DMA 传输 */
    SET_BIT(hspi1.Instance->CR2, SPI_CR2_RXDMAEN);
    SET_BIT(hspi1.Instance->CR2, SPI_CR2_TXDMAEN);

    encoder_isr_prepare();
#endif
    __HAL_SPI_ENABLE(&hspi1);

}

void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc)
{
	//static int value = 0;
	//gpio_set_value(&dbg_out2, value);
	//value = !value;
#ifdef TLE_5012B_DMA
	/* Step1：使能dma 传输 */
	gpio_set_value(&sen_cs, 1);
#endif
	//gpio_set_value(&dbg_out2, 0);
	runFoC();
	//gpio_set_value(&dbg_out2, 1);
	return;
}
extern float g_angleSpeed;
void app_main()
{
	gpio_set_value(&led_r, 1);
	delay_us(1000 * 1000);
	gpio_set_value(&led_r, 0);
	gpio_set_value(&led_g, 1);

	printf("app_main start ...\n\r");

	adc_init();		/* adc 采样 */
	spi_init();		/* spi dma 配置 */
	timer_init();	/* timer 配置 */
	drv832x_init();
	initFOC();
	while (1) {
		//dump_tle5012b();
		//double val = 0;
		//gpio_set_value(&dbg_out2, 0);
		//getSensorAngle(&val);
		//gpio_set_value(&dbg_out2, 1);
		//encoder_isr_prepare();
		//printf("angleSpeed = %f \n", g_angleSpeed);
		//printf("val = %f \n", val);

		delay_us(100 * 1000);
	}

	return;
}
